/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ahrs_auxiliary.cpp
  * @author     baiyang
  * @date       2021-11-22
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>
#include <stdint.h>

#include "ahrs_ekf.h"
#include <EKF/ekf.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>

#include <sensor_imu/sensor_imu.h>
#include <sensor_baro/sensor_baro.h>
#include <sensor_compass/sensor_compass.h>
#include <sensor_gps/sensor_gps.h>
#include <common/geo/declination.h>
#include <parameter/param.h>
#include <sensor_airspeed/sensor_airspeed.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void ahrs_ekf_try_set_origin(void);
static float filter_altitude_ellipsoid(float amsl_hgt);
/*----------------------------------variable----------------------------------*/
extern Ekf *_ekf;
extern parameters *_params;
extern int64_t _last_time_slip_us;

extern uint32_t _device_id_accel;
extern uint32_t _device_id_baro;
extern uint32_t _device_id_gyro;
extern uint32_t _device_id_mag;

extern int32_t _param_ekf2_aid_mask;

uint64_t _gps_time_usec;
int32_t _gps_alttitude_ellipsoid;///< altitude in 1E-3 meters (millimeters) above ellipsoid
uint64_t _gps_alttitude_ellipsoid_previous_timestamp; ///< storage for previous timestamp to compute dt
float   _wgs84_hgt_offset;  ///< height offset between AMSL and WGS84

uitc_vehicle_origin vehicle_origin;

matrix::Vector3f _last_accel_bias_published{};
matrix::Vector3f _last_gyro_bias_published{};
matrix::Vector3f _last_mag_bias_published{};
uint64_t _last_sensor_bias_published = 0;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       更新气压计数据
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_update_baro_sample()
{
    static uint64_t last_time_us = 0;
    uint64_t last_update_us = sensor_baro_get_last_update_usec();

    if (last_update_us != last_time_us && sensor_baro_healthy()) {
        _ekf->set_air_density(sensor_baro_get_air_density_ratio());

        float alt_m = sensor_baro_get_altitude_abs();

        bool baro_reset = false;
        if (_device_id_baro != sensor_baro_get_primary_id()) {
            _device_id_baro = sensor_baro_get_primary_id();
            baro_reset = true;
        }

        _ekf->setBaroData(baroSample{last_update_us, alt_m, baro_reset});

        last_time_us = last_update_us;
    }
}

void ahrs_ekf_update_compass_sample()
{
    static uint64_t last_time_us = 0;
    uint64_t last_update_us = sensor_compass_last_update_usec();

    if (last_update_us != last_time_us && sensor_compass_healthy()) {
        const Vector3f_t * mag = sensor_compass_get_field();

        bool mag_reset = false;
        if (_device_id_mag != sensor_compass_get_primary_id()) {
            _device_id_mag = sensor_compass_get_primary_id();
            mag_reset = true;
        }

        magSample mag_sample {
            .time_us = last_update_us,
            .mag = Vector3f{
                mag->x*0.001f,
                mag->y*0.001f,
                mag->z*0.001f},
            .reset = mag_reset
        };
        _ekf->setMagData(mag_sample);

        last_time_us = last_update_us;
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_update_gps_sample()
{
    static uint64_t last_time_us = 0;
    struct GPS_State* state = sensor_gps_primary_state();
    struct GPS_timing* timing = sensor_gps_primary_timing();

    if (timing->last_fix_time_us - last_time_us >= 70*1000) {
        gpsMessage gps_msg{
                .time_usec = timing->last_fix_time_us,
                .lat = state->location.lat,
                .lon = state->location.lng,
                .alt = state->location.alt*10,     // TOOD：alt注释是海拔高度，但融合过程似乎这个变量应该是gps测量的椭球高度？
                .yaw = state->have_gps_yaw?radians(state->gps_yaw):NAN,
                .yaw_offset = 0,
                .yaw_accuracy = state->have_gps_yaw_accuracy?radians(state->gps_yaw_accuracy):NAN,
                .fix_type = (uint8_t)state->status,
                .eph = state->horizontal_accuracy,
                .epv = state->vertical_accuracy,
                .sacc = state->speed_accuracy,
                .vel_m_s = state->ground_speed,
                .vel_ned = Vector3f{
                    state->velocity.x,
                    state->velocity.y,
                    state->velocity.z
                },
                .vel_ned_valid = state->have_horizontal_velocity,
                .nsats = state->num_sats,
                .pdop = sqrtf(state->hdop * 0.01f *state->hdop * 0.01f 
                          + state->vdop * 0.01f  * state->vdop * 0.01f ),
            };

        _ekf->setGpsData(gps_msg);

        _gps_time_usec = gps_msg.time_usec;
        _gps_alttitude_ellipsoid = state->location.alt*10;

        last_time_us = timing->last_fix_time_us;
    }
}

/**
  * @brief       更新空速计数据
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_update_airspeed_sample()
{
    static uint64_t last_time_us = 0;
    uint64_t last_update_us = sensor_as_last_update_us();

    if (last_update_us != last_time_us && sensor_as_use() && sensor_as_healthy()) {
        float  EAS2TAS = math_rsqrtf(sensor_baro_get_air_density_ratio());

        estimator::airspeedSample airspeed_sample {
            .time_us = last_update_us,
            .true_airspeed = sensor_as_get_airspeed() * EAS2TAS,
            .eas2tas = EAS2TAS,
        };
        _ekf->setAirspeedData(airspeed_sample);

        last_time_us = last_update_us;
    }
}

/**
  * @brief       
  * @param[in]   timestamp  
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_publish_attitude(const uint64_t timestamp)
{
    if (_ekf->attitude_valid()) {
        // generate vehicle attitude quaternion data
        uitc_vehicle_attitude vehicle_attitude;
        float q[4];
        _ekf->getQuaternion().copyTo(q);

        vehicle_attitude.timestamp_us = timestamp;
        vehicle_attitude.vehicle_quat.q0 = q[0];
        vehicle_attitude.vehicle_quat.q1 = q[1];
        vehicle_attitude.vehicle_quat.q2 = q[2];
        vehicle_attitude.vehicle_quat.q3 = q[3];

        quat_to_euler(&vehicle_attitude.vehicle_quat, &vehicle_attitude.vehicle_euler);

        itc_publish(ITC_ID(vehicle_attitude),&vehicle_attitude);
    }
}

void ahrs_ekf_publish_local_position(const uint64_t timestamp)
{
    uitc_vehicle_local_position lpos;
    // generate vehicle local position data
    lpos.timestamp_sample = timestamp;

    // Position of body origin in local NED frame
    const Vector3f position{_ekf->getPosition()};
    lpos.x = position(0);
    lpos.y = position(1);
    lpos.z = position(2);

    lpos.alt_valid = vehicle_origin.valid_alt;
    lpos.alt = -position(2) + vehicle_origin.alt;

    // Velocity of body origin in local NED frame (m/s)
    const Vector3f velocity{_ekf->getVelocity()};
    lpos.vx = velocity(0);
    lpos.vy = velocity(1);
    lpos.vz = velocity(2);

    // vertical position time derivative (m/s)
    lpos.z_deriv = _ekf->getVerticalPositionDerivative();

    uitc_sensor_acc sensor_acc;
    itc_copy_from_hub(ITC_ID(sensor_acc), &sensor_acc);

    const Vector3f accel_bias{_ekf->getAccelBias()};
    Vector3f_t acc_bf = {sensor_acc.sensor_acc_filter[0] - accel_bias(0),
                         sensor_acc.sensor_acc_filter[1] - accel_bias(1),
                         sensor_acc.sensor_acc_filter[2] - accel_bias(2)};

    uitc_vehicle_attitude vehicle_attitude;
    itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude);

    Vector3f_t acc_ef;
    quat_vec_rotate(&vehicle_attitude.vehicle_quat, &acc_ef, &acc_bf);

    // Acceleration of body origin in local frame
    lpos.ax = acc_ef.x;
    lpos.ay = acc_ef.y;
    lpos.az = acc_ef.z + GRAVITY_MSS;

    lpos.xy_valid = _ekf->local_position_is_valid();
    lpos.v_xy_valid = _ekf->local_position_is_valid();

    // TODO: some modules (e.g.: mc_pos_control) don't handle v_z_valid != z_valid properly
    lpos.z_valid = _ekf->isLocalVerticalPositionValid() || _ekf->isLocalVerticalVelocityValid();
    lpos.v_z_valid = _ekf->isLocalVerticalVelocityValid() || _ekf->isLocalVerticalPositionValid();

    // Position of local NED origin in GPS / WGS84 frame
    if (_ekf->global_origin_valid()) {
        lpos.ref_timestamp = _ekf->global_origin().getProjectionReferenceTimestamp();
        lpos.ref_lat = _ekf->global_origin().getProjectionReferenceLat(); // Reference point latitude in degrees
        lpos.ref_lon = _ekf->global_origin().getProjectionReferenceLon(); // Reference point longitude in degrees
        lpos.ref_alt = _ekf->getEkfGlobalOriginAltitude();           // Reference point in MSL altitude meters
        lpos.xy_global = true;
        lpos.z_global = true;

    } else {
        lpos.ref_timestamp = 0;
        lpos.ref_lat = static_cast<double>(NAN);
        lpos.ref_lon = static_cast<double>(NAN);
        lpos.ref_alt = NAN;
        lpos.xy_global = false;
        lpos.z_global = false;
    }

    Quatf delta_q_reset;
    _ekf->get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);

    lpos.heading = Eulerf(_ekf->getQuaternion()).psi();
    lpos.unaided_heading = _ekf->getUnaidedYaw();
    lpos.delta_heading = Eulerf(delta_q_reset).psi();
    lpos.heading_good_for_control = _ekf->isYawFinalAlignComplete();

    // Distance to bottom surface (ground) in meters
    // constrain the distance to ground to _rng_gnd_clearance
    lpos.dist_bottom = math::max(_ekf->getTerrainVertPos() - lpos.z, _params->rng_gnd_clearance);
    lpos.dist_bottom_valid = _ekf->isTerrainEstimateValid();
    lpos.dist_bottom_sensor_bitfield = _ekf->getTerrainEstimateSensorBitfield();

    _ekf->get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv);
    _ekf->get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);

    // get state reset information of position and velocity
    _ekf->get_posD_reset(&lpos.delta_z, &lpos.z_reset_counter);
    _ekf->get_velD_reset(&lpos.delta_vz, &lpos.vz_reset_counter);
    _ekf->get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
    _ekf->get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);

    lpos.dead_reckoning = _ekf->control_status_flags().inertial_dead_reckoning
                  || _ekf->control_status_flags().wind_dead_reckoning;

    // get control limit information
    _ekf->get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);

    // convert NaN to INFINITY
    if (!PX4_ISFINITE(lpos.vxy_max)) {
        lpos.vxy_max = INFINITY;
    }

    if (!PX4_ISFINITE(lpos.vz_max)) {
        lpos.vz_max = INFINITY;
    }

    if (!PX4_ISFINITE(lpos.hagl_min)) {
        lpos.hagl_min = INFINITY;
    }

    if (!PX4_ISFINITE(lpos.hagl_max)) {
        lpos.hagl_max = INFINITY;
    }

    // publish vehicle local position data
    lpos.timestamp = time_micros64();
    itc_publish(ITC_ID(vehicle_local_position), &lpos);

    ahrs_ekf_try_set_origin();
}

void ahrs_ekf_publish_global_position(const uint64_t timestamp)
{
    if (_ekf->global_position_is_valid()) {
        const Vector3f position{_ekf->getPosition()};

        // generate and publish global position data
        uitc_vehicle_global_position global_pos;
        global_pos.timestamp_sample = timestamp;

        // Position of local NED origin in GPS / WGS84 frame
        _ekf->global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);

        global_pos.alt = -position(2) + _ekf->getEkfGlobalOriginAltitude(); // Altitude AMSL in meters
        global_pos.alt_ellipsoid = filter_altitude_ellipsoid(global_pos.alt);

        // delta_alt, alt_reset_counter
        //  global altitude has opposite sign of local down position
        float delta_z = 0.f;
        uint8_t z_reset_counter = 0;
        _ekf->get_posD_reset(&delta_z, &z_reset_counter);
        global_pos.delta_alt = -delta_z;
        global_pos.alt_reset_counter = z_reset_counter;

        // lat_lon_reset_counter
        float delta_xy[2] {};
        uint8_t xy_reset_counter = 0;
        _ekf->get_posNE_reset(delta_xy, &xy_reset_counter);
        global_pos.lat_lon_reset_counter = xy_reset_counter;

        _ekf->get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);

        if (_ekf->isTerrainEstimateValid()) {
            // Terrain altitude in m, WGS84
            global_pos.terrain_alt = _ekf->getEkfGlobalOriginAltitude() - _ekf->getTerrainVertPos();
            global_pos.terrain_alt_valid = true;

        } else {
            global_pos.terrain_alt = NAN;
            global_pos.terrain_alt_valid = false;
        }

        global_pos.dead_reckoning = _ekf->control_status_flags().inertial_dead_reckoning
                        || _ekf->control_status_flags().wind_dead_reckoning;

        global_pos.timestamp = time_micros64();
        itc_publish(ITC_ID(vehicle_global_position), &global_pos);
    }
}

void ahrs_ekf_publish_innovations(const uint64_t timestamp)
{
    // publish estimator innovation data
    uitc_estimator_innovations innovations = {0};
    innovations.timestamp_sample = _ekf->get_imu_sample_delayed().time_us;
    _ekf->getGpsVelPosInnov(innovations.gps_hvel, innovations.gps_vvel, innovations.gps_hpos, innovations.gps_vpos);
    _ekf->getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos);
    _ekf->getBaroHgtInnov(innovations.baro_vpos);
    _ekf->getRngHgtInnov(innovations.rng_vpos);
    _ekf->getAuxVelInnov(innovations.aux_hvel);
    _ekf->getFlowInnov(innovations.flow);
    _ekf->getHeadingInnov(innovations.heading);
    _ekf->getMagInnov(innovations.mag_field);
    _ekf->getDragInnov(innovations.drag);
    _ekf->getAirspeedInnov(innovations.airspeed);
    _ekf->getBetaInnov(innovations.beta);
    _ekf->getHaglInnov(innovations.hagl);
    _ekf->getHaglRateInnov(innovations.hagl_rate);
    // Not yet supported
    innovations.aux_vvel = NAN;

    innovations.timestamp = timestamp;
    itc_publish(ITC_ID(estimator_innovations), &innovations);
}

void ahrs_ekf_publish_status(const uint64_t timestamp)
{
    uitc_estimator_status status = {0};
    status.timestamp_sample = _ekf->get_imu_sample_delayed().time_us;

    _ekf->getOutputTrackingError().copyTo(status.output_tracking_error);

    // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include
    // the GPS Fix bit, which is always checked)
    status.gps_check_fail_flags = _ekf->gps_check_fail_status().value & (((uint16_t)_params->gps_check_mask << 1) | 1);

    status.control_mode_flags = _ekf->control_status().value;
    status.filter_fault_flags = _ekf->fault_status().value;

    uint16_t innov_check_flags_temp = 0;
    _ekf->get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio,
                    status.vel_test_ratio, status.pos_test_ratio,
                    status.hgt_test_ratio, status.tas_test_ratio,
                    status.hagl_test_ratio, status.beta_test_ratio);

    // Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition
    // TODO: legacy use only, those flags are also in estimator_status_flags
    status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1);

    _ekf->get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy);
    _ekf->get_ekf_soln_status(&status.solution_status_flags);

    // reset counters
    status.reset_count_vel_ne = _ekf->state_reset_status().reset_count.velNE;
    status.reset_count_vel_d = _ekf->state_reset_status().reset_count.velD;
    status.reset_count_pos_ne = _ekf->state_reset_status().reset_count.posNE;
    status.reset_count_pod_d = _ekf->state_reset_status().reset_count.posD;
    status.reset_count_quat = _ekf->state_reset_status().reset_count.quat;

    status.time_slip = _last_time_slip_us * 1e-6f;

#if 0
    status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
    status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
    status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
    status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
#endif

    status.accel_device_id = _device_id_accel;
    status.baro_device_id = _device_id_baro;
    status.gyro_device_id = _device_id_gyro;
    status.mag_device_id = _device_id_mag;

    status.pre_flt_fail_mag_field_disturbed = _ekf->control_status_flags().mag_field_disturbed;

    status.timestamp = timestamp;
    itc_publish(ITC_ID(estimator_status), &status);
}

void ahrs_ekf_publish_wind_estimate(const uint64_t timestamp)
{
    if (_ekf->get_wind_status()) {
        // Publish wind estimate only if ekf declares them valid
        uitc_wind wind = {0};
        wind.timestamp_sample = _ekf->time_delayed_us();

        const Vector2f wind_vel = _ekf->getWindVelocity();
        const Vector2f wind_vel_var = _ekf->getWindVelocityVariance();
        _ekf->getAirspeedInnov(wind.tas_innov);
        _ekf->getAirspeedInnovVar(wind.tas_innov_var);
        _ekf->getBetaInnov(wind.beta_innov);
        _ekf->getBetaInnovVar(wind.beta_innov_var);

        wind.windspeed_north = wind_vel(0);
        wind.windspeed_east = wind_vel(1);
        wind.variance_north = wind_vel_var(0);
        wind.variance_east = wind_vel_var(1);
        wind.timestamp_us = timestamp;

        itc_publish(ITC_ID(wind), &wind);
    }
}

void ahrs_ekf_publish_sensor_bias(const uint64_t timestamp)
{
    // estimator_sensor_bias
    const Vector3f gyro_bias{_ekf->getGyroBias()};
    const Vector3f accel_bias{_ekf->getAccelBias()};
    const Vector3f mag_bias{_ekf->getMagBias()};

    // publish at ~1 Hz, or sooner if there's a change
    if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f)
        || (accel_bias - _last_accel_bias_published).longerThan(0.001f)
        || (mag_bias - _last_mag_bias_published).longerThan(0.001f)
        || (timestamp >= _last_sensor_bias_published + 1000000)) {

        uitc_estimator_sensor_bias bias = {0};
        bias.timestamp_sample = _ekf->time_delayed_us();

        // take device ids from sensor_selection_s if not using specific vehicle_imu_s
        if ((_device_id_gyro != 0) && (_params->imu_ctrl & static_cast<int32_t>(estimator::ImuCtrl::GyroBias))) {
            const Vector3f bias_var{_ekf->getGyroBiasVariance()};

            bias.gyro_device_id = _device_id_gyro;
            gyro_bias.copyTo(bias.gyro_bias);
            bias.gyro_bias_limit = _ekf->getGyroBiasLimit();
            bias_var.copyTo(bias.gyro_bias_variance);
            bias.gyro_bias_valid = bias_var.longerThan(0.0f) && !bias_var.longerThan(0.1f);
            bias.gyro_bias_stable = true;
            _last_gyro_bias_published = gyro_bias;
        }

        if ((_device_id_accel != 0) && (_params->imu_ctrl & static_cast<int32_t>(estimator::ImuCtrl::AccelBias))) {
            const Vector3f bias_var{_ekf->getAccelBiasVariance()};

            bias.accel_device_id = _device_id_accel;
            accel_bias.copyTo(bias.accel_bias);
            bias.accel_bias_limit = _ekf->getAccelBiasLimit();
            bias_var.copyTo(bias.accel_bias_variance);
            bias.accel_bias_valid = bias_var.longerThan(0.0f) && !bias_var.longerThan(0.1f);
            bias.accel_bias_stable = true;  //_accel_cal.cal_available;
            _last_accel_bias_published = accel_bias;
        }

        if (_device_id_mag != 0) {
            const Vector3f bias_var{_ekf->getMagBiasVariance()};

            bias.mag_device_id = _device_id_mag;
            mag_bias.copyTo(bias.mag_bias);
            bias.mag_bias_limit = _ekf->getMagBiasLimit();
            bias_var.copyTo(bias.mag_bias_variance);
            bias.mag_bias_valid = bias_var.longerThan(0.0f) && !bias_var.longerThan(0.1f);
            bias.mag_bias_stable = true;  //_mag_cal.cal_available;
            _last_mag_bias_published = mag_bias;
        }

        bias.timestamp = time_micros64();
        itc_publish(ITC_ID(estimator_sensor_bias), &bias);

        _last_sensor_bias_published = bias.timestamp;
    }
}

void ahrs_ekf_publish_yaw_estimator_status(const uint64_t timestamp)
{
    static_assert(sizeof(uitc_yaw_estimator_status::yaw) / sizeof(float) == N_MODELS_EKFGSF,
              "yaw_estimator_status_s::yaw wrong size");

    uitc_yaw_estimator_status yaw_est_test_data;

    if (_ekf->getDataEKFGSF(&yaw_est_test_data.yaw_composite, &yaw_est_test_data.yaw_variance,
                   yaw_est_test_data.yaw,
                   yaw_est_test_data.innov_vn, yaw_est_test_data.innov_ve,
                   yaw_est_test_data.weight)) {

        yaw_est_test_data.yaw_composite_valid = _ekf->isYawEmergencyEstimateAvailable();
        yaw_est_test_data.timestamp_sample = _ekf->time_delayed_us();
        yaw_est_test_data.timestamp = time_micros64();

        itc_publish(ITC_ID(yaw_estimator_status), &yaw_est_test_data);
    }
}

/**
  * @brief       
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_try_set_origin(void)
{
    float ref_alt;
    uint64_t origin_time;
    double ekf_origin_lat;
    double ekf_origin_lon;
    estimator::ekf_solution_status_u soln_status;

    const bool ekf_origin_valid = _ekf->getEkfGlobalOrigin(origin_time, ekf_origin_lat, ekf_origin_lon, ref_alt);
    _ekf->get_ekf_soln_status((uint16_t*)&soln_status);

    if (!vehicle_origin.valid_alt && (soln_status.flags.pos_vert_abs || soln_status.flags.pos_vert_agl)) {
        vehicle_origin.timestamp_us = time_micros64();
        vehicle_origin.alt = sensor_baro_get_altitude_abs() + _ekf->getPosition()(2); //_ekf->getPosition()(2)朝下为正，所以这里是+
        vehicle_origin.z   = 0.0f;
        vehicle_origin.valid_alt = true;
        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    if (ekf_origin_valid && !vehicle_origin.valid_hpos) {
        vehicle_origin.timestamp_us = time_micros64();
        vehicle_origin.lat = ekf_origin_lat * 1e7;
        vehicle_origin.lon = ekf_origin_lon * 1e7;

        vehicle_origin.x = _ekf->getPosition()(0);
        vehicle_origin.y = _ekf->getPosition()(1);

        vehicle_origin.yaw = Eulerf(_ekf->getQuaternion()).psi();
        vehicle_origin.mag_decl = geo_get_declination(ekf_origin_lat, ekf_origin_lon);
        vehicle_origin.valid_hpos = true;

        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    if (!vehicle_origin.valid_lpos && vehicle_origin.valid_hpos && vehicle_origin.valid_alt) {
        vehicle_origin.timestamp_us = time_micros64();
        vehicle_origin.valid_lpos = true;
        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }
}

static float filter_altitude_ellipsoid(float amsl_hgt)
{
    float height_diff = static_cast<float>(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt;

    if (_gps_alttitude_ellipsoid_previous_timestamp == 0) {

        _wgs84_hgt_offset = height_diff;
        _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;

    } else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) {

        // apply a 10 second first order low pass filter to baro offset
        float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp);
        _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec;
        float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset);
        _wgs84_hgt_offset += dt * constrain(offset_rate_correction, -0.1f, 0.1f);
    }

    return amsl_hgt + _wgs84_hgt_offset;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void ahrs_ekf_assign_param()
{
    if (_params == nullptr) {
        return;
    }

    param_link_variable(PARAM_ID(EKF2, EKF2_GYR_NOISE), &_params->gyro_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_ACC_NOISE), &_params->accel_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_GYR_B_NOISE), &_params->gyro_bias_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_ACC_B_NOISE), &_params->accel_bias_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_E_NOISE), &_params->mage_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_B_NOISE), &_params->magb_p_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_WIND_NOISE), &_params->wind_vel_nsd);
    param_link_variable(PARAM_ID(EKF2, EKF2_GND_EFF_DZ), &_params->gnd_effect_deadzone);
    param_link_variable(PARAM_ID(EKF2, EKF2_GND_MAX_HGT), &_params->gnd_effect_max_hgt);
    param_link_variable(PARAM_ID(EKF2, EKF2_HEAD_NOISE), &_params->mag_heading_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_NOISE), &_params->mag_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_EAS_NOISE), &_params->eas_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_BETA_NOISE), &_params->beta_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_GPS_V_NOISE), &_params->gps_vel_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_GPS_P_NOISE), &_params->gps_pos_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_NOAID_NOISE), &_params->pos_noaid_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_BARO_NOISE), &_params->baro_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_GYR_B_LIM), &_params->gyro_bias_lim);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_DECL), &_params->mag_declination_deg);
    param_link_variable(PARAM_ID(EKF2, EKF2_DECL_TYPE), &_params->mag_declination_source);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_TYPE), &_params->mag_fusion_type);
    param_link_variable(PARAM_ID(EKF2, EKF2_GPS_CHECK), &_params->gps_check_mask);
    param_link_variable(PARAM_ID(EKF2, EKF2_AID_MASK), &_param_ekf2_aid_mask);
    param_link_variable(PARAM_ID(EKF2, EKF2_BARO_CTRL), &_params->baro_ctrl);
    param_link_variable(PARAM_ID(EKF2, EKF2_EV_CTRL), &_params->ev_ctrl);
    param_link_variable(PARAM_ID(EKF2, EKF2_GPS_CTRL), &_params->gnss_ctrl);
    param_link_variable(PARAM_ID(EKF2, EKF2_RNG_CTRL), &_params->rng_ctrl);
    param_link_variable(PARAM_ID(EKF2, EKF2_HGT_REF), &_params->height_sensor_ref);
    param_link_variable(PARAM_ID(EKF2, EKF2_TERR_MASK), &_params->terrain_fusion_mode);
    param_link_variable(PARAM_ID(EKF2, EKF2_RNG_NOISE), &_params->range_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_EVV_GATE), &_params->ev_vel_innov_gate);
    param_link_variable(PARAM_ID(EKF2, EKF2_EVP_GATE), &_params->ev_pos_innov_gate);
    param_link_variable(PARAM_ID(EKF2, EKF2_OF_N_MIN), &_params->flow_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_OF_N_MAX), &_params->flow_noise_qual_min);
    param_link_variable(PARAM_ID(EKF2, EKF2_ARSP_THR), &_params->arsp_thr);
    param_link_variable(PARAM_ID(EKF2, EKF2_DRAG_NOISE), &_params->drag_noise);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_CHECK), &_params->mag_check);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_CHK_STR), &_params->mag_check_strength_tolerance_gs);
    param_link_variable(PARAM_ID(EKF2, EKF2_MAG_CHK_INC), &_params->mag_check_inclination_tolerance_deg);
    param_link_variable(PARAM_ID(EKF2, EKF2_SYNT_MAG_Z), &_params->synthesize_mag_z);

    _params->sensor_interval_max_ms = 100;  // 气压计和磁罗盘数据更新频率的最大值
}

/*------------------------------------test------------------------------------*/


